% Programmed by Shayan Hosseinian & Elia Deylami

clc;
clear;
close all;

disp('--- Four-Bar Mechanism Analysis ---');
L2 = input('Enter length of crank (Link 2): ');
L3 = input('Enter length of coupler (Link 3): ');
L4 = input('Enter length of rocker (Link 4): ');
L1 = input('Enter length of fixed link (Link 1): ');
w2 = input('Enter angular velocity of crank (rad/s): ');

r_p = input('Distance of point P on coupler: ');
delta = deg2rad(input('Angle offset of point P from link 3 (in degrees): '));

theta2 = linspace(0, 2*pi, 360);
dt = (2*pi)/(w2 * 360);
t = 0:dt:(length(theta2)-1)*dt;

theta3 = zeros(size(theta2));
theta4 = zeros(size(theta2));
px = zeros(size(theta2));
py = zeros(size(theta2));

A = [0, 0];
D = [L1, 0];

for i = 1:length(theta2)
    th2 = theta2(i);
    B = A + L2 * [cos(th2), sin(th2)];
    R = D - B;
    R_len = norm(R);

    if R_len > (L3 + L4) || R_len < abs(L3 - L4)
        continue;
    end

    cos_phi = (L3^2 + R_len^2 - L4^2) / (2 * L3 * R_len);
    phi = acos(min(max(cos_phi, -1), 1)); 
    alpha = atan2(R(2), R(1));
    th3 = alpha - phi;  
    theta3(i) = th3;

    C = B + L3 * [cos(th3), sin(th3)];

    rDC = C - D;
    th4 = atan2(rDC(2), rDC(1));
    theta4(i) = th4;

    Px = B(1) + r_p*cos(th3 + delta);
    Py = B(2) + r_p*sin(th3 + delta);
    px(i) = Px;
    py(i) = Py;

    figure(1); clf; hold on; axis equal;
    plot([A(1), B(1)], [A(2), B(2)], 'ro-', 'LineWidth', 2); 
    plot([B(1), C(1)], [B(2), C(2)], 'go-', 'LineWidth', 2); 
    plot([C(1), D(1)], [C(2), D(2)], 'bo-', 'LineWidth', 2); 
    plot([A(1), D(1)], [A(2), D(2)], 'k--', 'LineWidth', 1); 
    plot(Px, Py, 'mo', 'MarkerFaceColor', 'm');              

    plot([A(1), B(1), C(1), D(1)], [A(2), B(2), C(2), D(2)], 'ko', 'MarkerFaceColor','k');

    xlim([-L3-2, L1+L4+2]); ylim([-L3-2, L3+2]);
    title(sprintf('Corrected Four-Bar Mechanism | Frame %d', i));
    drawnow;
    pause(0.01);
end

figure(2);
plot(px, py, 'm');
xlabel('x'); ylabel('y');
title('Coupler Point Trajectory (Point P)');
axis equal; grid on;